#ifndef READ_IMU_H
#define READ_IMU_H

#include "ros/ros.h"
#include <iostream>
#include <serial/serial.h>
#include <serial/v8stdint.h>
#include <stdio.h>
#include <string>
#include <vector>
#include <thread>
#include <boost/bind.hpp>


namespace imu_serial
{

struct IMU_DATA {
  double Ax;    // Accel x axis
  double Ay;    // Accel y axis
  double Az;    // Accel z axis
  double Gx;    // Gyro x axis
  double Gy;    // Gyro y axis
  double Gz;    // Gyro z axis
  double Roll;  // Roll
  double Pitch; // Pitch
  double Yaw;   // Yaw
  double Temp;  // Temperature
};

class read_imu
{
public:
    read_imu();
    ~read_imu();

    bool connect(ros::NodeHandle &nh);

    void read_buffer();

    bool ParsePacketB4(IMU_DATA *imuData, std::vector<uint8_t> buffer);

public:
    std::ostringstream os;
    serial::Serial ImuSerial;
    std::string port;
    int baudrate;
    int timeout;
    int frequency;

    std::thread read_serial_thread;
    bool is_connect_serial;

    int numlen;
    std::vector<uint8_t> USER_TX_BUF;
    std::vector<uint8_t> USER_RX_BUF;

    IMU_DATA *imuDataRead = nullptr;

    const int Sensor_Scale = 65536;
    const int Angle_Scale = 360;
    const int Accel_Scale = 20;
    const int Rate_Scale = 1260;
    const int Temp_Scale = 200;
    const int MinLength = 20;

};

}


#endif